#!/usr/bin/env python
"""Control the rudder servo

Subscribes: rudder_control (Int16)
"""
import time
import pigpio
import rospy
from std_msgs.msg import UInt16, Int16
import numpy as np

rudderdata = rospy.get_param('rudder')
rudderservo_PWM_offset = rudderdata['PWMoffset']
rudderservo_lower_limits = rudderdata['servolowerlimits']
rudderservo_higher_limits = rudderdata['servohigherlimits']
rudderservo_netural_point = (rudderservo_lower_limits +
                             rudderservo_higher_limits) / 2
rudderservo_range = (rudderservo_higher_limits - rudderservo_lower_limits)

PIN = rospy.get_param('rudder/pin')

def setup():
    pi = pigpio.pi()
    pi.set_mode(13, pigpio.OUTPUT) # GPIO 13/RPi PIN 33 as rudder servo pin

def rudderservoPWMcontrol(data):
    """This function takes in the /rudder_control (90 to -90) value and directly write PWM signal to the rudder servo. Netural point was determined by the start and end points. rudderservoPWMoffset is used for software level trim. """
    degrees = data.data
    pwm = rudderservo_range*(-1.0*degrees)/90 + rudderservo_netural_point +\
         rudderservo_PWM_offset
    pi.set_servo_pulsewidth(PIN, pwm)

def post():
    '''Power-On Self Test'''
    if not rospy.get_param('do_post', False):
        return

    rospy.logwarn('rudder test: lower limit')
    for _ in range(4):
        pi.set_servo_pulsewidth(PIN, rudderservo_netural_point)
        time.sleep(0.25)
        pi.set_servo_pulsewidth(PIN, rudderservo_lower_limits)
        time.sleep(0.25)

    rospy.logwarn('rudder test: higher limit')
    for _ in range(4):
        pi.set_servo_pulsewidth(PIN, rudderservo_netural_point)
        time.sleep(0.25)
        pi.set_servo_pulsewidth(PIN, rudderservo_higher_limits)
        time.sleep(0.25)

if __name__ == '__main__':
    pi = pigpio.pi()
    post()
    try:
        rospy.init_node('actuator_driver_servos', anonymous=True)
        rospy.Subscriber('rudder_control', Int16, rudderservoPWMcontrol)
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
